#include <Arduino.h>

// 定义DRV8833控制引脚
const int AIN1 = 18;  // 电机A PWM引脚
const int AIN2 = 19;  // 电机A PWM引脚
const int BIN1 = 4;   // 电机B PWM引脚
const int BIN2 = 5;   // 电机B PWM引脚
const int nSLEEP = 34; // 使能引脚（已确认接3.3V）

// PWM设置（优化参数）
const int freq = 10000;     // 提高PWM频率到10kHz（减少电机噪音）
const int resolution = 10;  // 10位分辨率（0-1023），比8位更精细

// 小车速度设置 (0-100)
const int DEFAULT_SPEED = 90; // 提高默认速度到90%
const int MAX_SPEED = 100;    // 最大速度限制

// 电机死区补偿（解决低速启动问题）
const int DEADZONE_COMPENSATE = 5; // 补偿值（根据电机特性调整）

void forward(int speed);
void setMotorA(int speed);
void setMotorB(int speed);

void setup() {
  // 配置PWM（优化后的设置）
  ledcSetup(0, freq, resolution); // 通道0用于AIN1
  ledcSetup(1, freq, resolution); // 通道1用于AIN2
  ledcSetup(2, freq, resolution); // 通道2用于BIN1
  ledcSetup(3, freq, resolution); // 通道3用于BIN2
  
  // 绑定PWM通道到GPIO
  ledcAttachPin(AIN1, 0);
  ledcAttachPin(AIN2, 1);
  ledcAttachPin(BIN1, 2);
  ledcAttachPin(BIN2, 3);
  
  // 配置使能引脚
  pinMode(nSLEEP, OUTPUT);
  digitalWrite(nSLEEP, HIGH); // 启用驱动器
  
  Serial.begin(115200);
  Serial.println("ESP32小车控制已就绪（优化版）");
  
  // 启动小车前进（带加速渐变）
  for (int i = 0; i <= DEFAULT_SPEED; i += 5) {
    forward(i);
    delay(50);
  }
}

void loop() {
  // 主循环可以添加其他控制逻辑
  delay(100); // 防止 watchdog 触发
}

// 改进的电机控制函数（带死区补偿）
void setMotorA(int speed) {
  speed = constrain(speed, -MAX_SPEED, MAX_SPEED);
  
  // 死区补偿（解决电机启动问题）
  if (speed > 0 && speed < DEADZONE_COMPENSATE) {
    speed = DEADZONE_COMPENSATE;
  } else if (speed < 0 && speed > -DEADZONE_COMPENSATE) {
    speed = -DEADZONE_COMPENSATE;
  }
  
  if(speed > 0) {
    ledcWrite(0, map(speed, 0, 100, 0, (1<<resolution)-1)); // 使用位运算计算最大值
    ledcWrite(1, 0);
  } else if(speed < 0) {
    ledcWrite(0, 0);
    ledcWrite(1, map(-speed, 0, 100, 0, (1<<resolution)-1));
  } else {
    ledcWrite(0, 0);
    ledcWrite(1, 0);
  }
}

// 电机B控制（同A电机）
void setMotorB(int speed) {
  speed = constrain(speed, -MAX_SPEED, MAX_SPEED);
  
  if (speed > 0 && speed < DEADZONE_COMPENSATE) {
    speed = DEADZONE_COMPENSATE;
  } else if (speed < 0 && speed > -DEADZONE_COMPENSATE) {
    speed = -DEADZONE_COMPENSATE;
  }
  
  if(speed > 0) {
    ledcWrite(2, map(speed, 0, 100, 0, (1<<resolution)-1));
    ledcWrite(3, 0);
  } else if(speed < 0) {
    ledcWrite(2, 0);
    ledcWrite(3, map(-speed, 0, 100, 0, (1<<resolution)-1));
  } else {
    ledcWrite(2, 0);
    ledcWrite(3, 0);
  }
}

// 运动控制函数（增加渐变控制）
void forward(int speed) {
  speed = constrain(speed, 0, MAX_SPEED);
  static int lastSpeed = 0;
  
  // 速度渐变（减少电流冲击）
  if (abs(speed - lastSpeed) > 20) {
    int step = (speed > lastSpeed) ? 5 : -5;
    for (int s = lastSpeed; s != speed; s += step) {
      setMotorA(s);
      setMotorB(s);
      delay(30);
    }
  }
  
  setMotorA(speed);
  setMotorB(speed);
  lastSpeed = speed;
  
  Serial.print("前进，速度：");
  Serial.println(speed);
}



// 小车后退
void backward(int speed) {
  Serial.print("后退，速度：");
  Serial.println(speed);
  setMotorA(-speed);
  setMotorB(-speed);
}

// 小车左转
void turnLeft(int speed) {
  Serial.print("左转，速度：");
  Serial.println(speed);
  setMotorA(-speed); // 左轮后退
  setMotorB(speed);  // 右轮前进
}

// 小车右转
void turnRight(int speed) {
  Serial.print("右转，速度：");
  Serial.println(speed);
  setMotorA(speed);  // 左轮前进
  setMotorB(-speed); // 右轮后退
}

// 小车停止
void stopCar() {
  Serial.println("停止");
  setMotorA(0);
  setMotorB(0);
}